import os
import pandas as pd
import matplotlib.pyplot as plt
# import numpy as np




# ---------------------------------
#           身躯的姿态角
# ---------------------------------

# # 读取数据
# data = pd.read_csv('saveCtl(1).txt', delimiter=',', header=None)

# time = data.index * 0.001  # 转换为秒
# Body_orientation = data.iloc[:, 12:15] 

# labels = [
#     'Roll',
#     'Pitch',
#     'Yaw',
# ]

# ylabels = [ 
#            'roll (rad)',
#            'pitch (rad)',
#            'yaw (rad)',
#            ]


# # # 大图
# # fig, axs = plt.subplots(3, 1, figsize=(12, 18))  # 3 行 1 列的子图
# # for i in range(3):
# #     # axs.T.flat[i].plot(time, Body_orientation.iloc[:, i], label=labels[i])
# #     axs.T.flat[i].plot(time[0:24000], Body_orientation.iloc[15000:39000, i], label=labels[i])
# #     axs.T.flat[i].set_xlabel('time (s)', fontsize=28)
# #     axs.T.flat[i].set_ylabel(ylabels[i], fontsize=28)
# #     # axs.T.flat[i].legend(fontsize=28)
# #     axs.T.flat[i].grid()
# #     axs.T.flat[i].tick_params(axis='both', which='major', labelsize=28)  # 控制横纵坐标轴刻度的字体大小

# # # plt.suptitle('Body Orientation in Euler Angles', fontsize=26)
# # plt.tight_layout(rect=[0, 0, 1, 0.96])  # 调整布局以避免标题和子图重叠
# # plt.savefig('Body Orientation in Euler Angles.eps',dpi=600,format='eps')


# # 小图
# import matplotlib.ticker as mticker  # 引入ticker模块
# for i in range(3):
#     fig, ax = plt.subplots(figsize=(12, 6))  # 为每个子图创建单独的图
#     ax.yaxis.set_major_formatter(mticker.FormatStrFormatter('%.2f'))
#     ax.plot(time[0:24000], Body_orientation.iloc[15000:39000, i])
#     ax.set_xlabel('time (s)', fontsize=28)
#     ax.set_ylabel(ylabels[i], fontsize=28)
#     ax.grid()
#     ax.tick_params(axis='both', which='major', labelsize=28)
    
#     # 保存单独的子图
#     plt.tight_layout()
#     plt.savefig(f'Body Orientation {labels[i]}.eps', dpi=600, format='eps')



# ---------------------------------
#      质心的目标速度和实际速度
# ---------------------------------

# # 读取数据
# data = pd.read_csv('mpc_record(1).txt', delimiter=',', header=None)

# # 创建一个子图
# fig, ax = plt.subplots(figsize=(12, 6))  # 单个子图，调整大小

# time = data.index * 0.001  # 转换为秒
# actual_velocity = data.iloc[:, 52]  # 获取第52列（索引为51）

# # 质心的目标位置曲线
# desired_velocity = np.zeros(data.index.size)

# # 手动设置曲线变化的时刻
# change_points = [16500, 21800, 28400, 35000]  # 对应13秒, 16.3秒, 20秒, 25秒
# desired_velocity[change_points[0]:change_points[1]] = 0.1  # 13秒到16.3秒之间值为0.1
# desired_velocity[change_points[1]:change_points[2]] = 0.2  # 16.3秒到20秒之间值为0.2
# desired_velocity[change_points[2]:change_points[3]] = 0.3  # 20秒到25秒之间值为0.3


# # 绘图
# ax.plot(time[:24000], desired_velocity[15000:39000], label='desired velocity', color='r')
# ax.plot(time[:24000], actual_velocity[15000:39000], label='actual velocity', color=(0.0, 0.447, 0.741))
# # ax.plot(time, desired_velocity, label='Desired Velocity', color='r')
# # ax.plot(time, actual_velocity, label='Actual Velocity', color=(0.0, 0.447, 0.741))
# ax.set_xlabel('time (s)', fontsize=28)
# ax.set_ylabel('velocity X (m/s)', fontsize=28)
# ax.legend(fontsize=20)
# ax.grid()
# ax.tick_params(axis='both', which='major', labelsize=28)  # 改变横纵坐标刻度的字体大小
# plt.tight_layout()  # 自动调整布局
# plt.savefig('Velocity Tracking.eps',dpi=600,format='eps')



# ---------------------------------
#        质心位置和质心速度
# ---------------------------------

# # 读取数据
# data = pd.read_csv('mpc_record(1).txt', delimiter=',', header=None)


# time = data.index * 0.001  # 转换为秒
# COM_data = data.iloc[:, 49:55]

# labels = [
#     'X',
#     'Y',
#     'Z',
#     'X',
#     'Y',
#     'Z'
# ]


# titles = [
#     'Body CoM Position',
#     'Body CoM Velocity'
# ]

# ylabels = [ 
#            'position X (m)',
#            'position Y (m)',
#            'position Z (m)',
#            'velocity X (m/s)',
#            'velocity Y (m/s)',
#            'velocity Z (m/s)']

# # 大图  
# # fig, axs = plt.subplots(3, 2, figsize=(12, 18))  # 3 行 2 列的子图
# # for i in range(6):
# #     # axs.T.flat[i].plot(time, COM_data.iloc[:, i], label=labels[i])
# #     axs.T.flat[i].plot(time[:24000], COM_data.iloc[15000:39000, i], label=labels[i])
# #     if i == 0:
# #         axs.T.flat[i].set_title(titles[0], fontsize=28)
# #     if i == 3:
# #         axs.T.flat[i].set_title(titles[1], fontsize=28)
# #     axs.T.flat[i].set_xlabel('time (s)', fontsize=28)
# #     axs.T.flat[i].set_ylabel(ylabels[i], fontsize=28)
# #     # axs.T.flat[i].legend(fontsize=20)
# #     axs.T.flat[i].grid()
# #     axs.T.flat[i].tick_params(axis='both', which='major', labelsize=28)  # 改变横纵坐标刻度的字体大小
# #     # axs.T.flat[i].set_ylim([-2, 2])
# #     plt.tight_layout()

# # plt.savefig('Plots of Body CoM Position and Velocity.eps',dpi=600,format='eps')


# # 小图
# for i in range(6):
#     fig, axs = plt.subplots(figsize=(12, 6))  # 为每个子图创建单独的图
#     axs.plot(time[:24000], COM_data.iloc[15000:39000, i], label=labels[i])
#     axs.set_xlabel('time (s)', fontsize=28)
#     axs.set_ylabel(ylabels[i], fontsize=28)
#     # axs.legend(fontsize=20)
#     axs.grid()
#     axs.tick_params(axis='both', which='major', labelsize=28)  # 改变横纵坐标刻度的字体大小
#     plt.tight_layout()
#     if i < 3:
#         plt.savefig(f'Plots of Body CoM Position {labels[i]}.eps',dpi=600,format='eps')
#     else:
#         plt.savefig(f'Plots of Body CoM Velocity {labels[i]}.eps',dpi=600,format='eps')



# ---------------------------------
#        腿部关节力矩跟踪
# ---------------------------------

# # 读取数据
# data = pd.read_csv('data/record_data.txt', delimiter=',', header=None)

# time = data.index * 0.001  # 转换为秒

# # left_leg
# # traget_torque = data.iloc[int(0.0/0.001):, 31:37]
# # actual_torque = data.iloc[int(0.0/0.001):, 19:25]

# # right_leg
# traget_torque = data.iloc[int(0.0/0.001):, 25:31]
# actual_torque = data.iloc[int(0.0/0.001):, 13:19]

# labels = [
#     'Joint1',
#     'Joint2',
#     'Joint3',
#     'Joint4',
#     'Joint5',
#     'Joint6'
# ]

# labels1 = [
#     'traget torque joint1',
#     'traget torque joint2',
#     'traget torque joint3',
#     'traget torque joint4',
#     'traget torque joint5',
#     'traget torque joint6'
# ]


# labels2 = [
#     'actual torque joint1',
#     'actual torque joint2',
#     'actual torque joint3',
#     'actual torque joint4',
#     'actual torque joint5',
#     'actual torque joint6'
# ]

# # fig, axs = plt.subplots(3, 2, figsize=(12, 18)) 
# # for i in range(6):
# #     axs.T.flat[i].plot(time[:24000], traget_torque.iloc[15000:39000, i], label=labels1[i])
# #     axs.T.flat[i].plot(time[:24000], actual_torque.iloc[15000:39000, i], label=labels2[i])
# #     # axs.T.flat[i].plot(time, traget_torque.iloc[:, i], label=labels1[i])
# #     # axs.T.flat[i].plot(time, actual_torque.iloc[:, i], label=labels2[i])
# #     # axs.T.flat[i].set_title(titles[i])
# #     axs.T.flat[i].set_xlabel('time (s)', fontsize=28)
# #     axs.T.flat[i].set_ylabel('torque (N·m)', fontsize=28)
# #     axs.T.flat[i].legend( fontsize=16)
# #     axs.T.flat[i].grid()
# #     # axs.T.flat[i].set_ylim([-2, 2])
# #     axs.T.flat[i].tick_params(axis='both', which='major', labelsize=28)  # 改变横纵坐标刻度的字体大小

# # # plt.suptitle('Left Leg Joint Torque', fontsize=16)
# # # plt.suptitle('Right Leg Joint Torque Tracking', fontsize=16)
# # plt.tight_layout(rect=[0, 0, 1, 0.96])  # 调整布局以避免标题和子图重叠
# # plt.savefig('Right Leg Joint Torque Tracking.eps',dpi=600,format='eps')



# # 小图
# for i in range(6):
#     fig, axs = plt.subplots(figsize=(12, 6))  # 为每个子图创建单独的图
#     axs.plot(time[:24000], traget_torque.iloc[15000:39000, i], label=labels1[i])
#     axs.plot(time[:24000], actual_torque.iloc[15000:39000, i], label=labels2[i])
#     axs.set_xlabel('time (s)', fontsize=28)
#     axs.set_ylabel('torque (N·m)', fontsize=28)
#     axs.legend(fontsize=16)
#     axs.grid()
#     axs.tick_params(axis='both', which='major', labelsize=28)  # 改变横纵坐标刻度的字体大小
#     plt.tight_layout()  # 调整布局以避免标题和子图重叠
#     plt.savefig(f'Right Leg {labels[i]} Torque Tracking.eps',dpi=600,format='eps')



# ---------------------------------
#           足端力和力矩
# ---------------------------------

# # 读取数据
# data = pd.read_csv('saveCtl(1).txt', delimiter=',', header=None)

# time = data.index * 0.001  # 转换为秒
# # MPC_data = data.iloc[:, 0:6] # left leg
# MPC_data = data.iloc[:, 6:12] #right leg

# labels = [
#     'X',
#     'Y',
#     'Z',
#     'X',
#     'Y',
#     'Z'
# ]


# ylabels = [ 
#            'force X (N)',
#            'force Y (N)',
#            'force Z (N)',
#            'torque X (N·m)',
#            'torque Y (N·m)',
#            'torque Z (N·m)']

# titles = [
#         'MPC Forces',
#         'MPC Moments'
#             ]

# # # 大图
# # fig, axs = plt.subplots(3, 2, figsize=(12, 18))  # 3 行 2 列的子图
# # for i in range(6):
# #     axs.T.flat[i].plot(time[0:24000], MPC_data.iloc[15000:39000, i], label=labels[i])
# #     # axs.T.flat[i].plot(time, MPC_data.iloc[:, i], label=labels[i])
# #     if i == 0:
# #         axs.T.flat[i].set_title(titles[0], fontsize=28)
# #     if i == 3:
# #        axs.T.flat[i].set_title(titles[1], fontsize=28)
# #     axs.T.flat[i].set_xlabel('time (s)', fontsize=28)
# #     axs.T.flat[i].set_ylabel(ylabels[i], fontsize=28)
# #     # axs.T.flat[i].legend()
# #     axs.T.flat[i].grid()
# #     axs.T.flat[i].tick_params(axis='both', which='major', labelsize=28)  # 改变横纵坐标刻度的字体大小

# # # plt.suptitle('MPC Forces and Moments of Right Leg', fontsize=16)
# # plt.tight_layout(rect=[0, 0, 1, 0.96])  # 调整布局以避免标题和子图重叠
# # plt.savefig('Plots of MPC Force and Moment in Flat Terrain.eps',dpi=600,format='eps')


# # 小图
# for i in range(6):
#     fig, axs = plt.subplots(figsize=(12, 6))  # 为每个子图创建单独的图
#     axs.plot(time[0:24000], MPC_data.iloc[15000:39000, i], label=labels[i])
#     axs.set_xlabel('time (s)', fontsize=28)
#     axs.set_ylabel(ylabels[i], fontsize=28)
#     axs.grid()
#     axs.tick_params(axis='both', which='major', labelsize=28)  # 改变横纵坐标刻度的字体大小
#     plt.tight_layout()  # 调整布局以避免标题和子图重叠
#     if i < 3:
#         plt.savefig(f'Plots of MPC Force {labels[i]} in Flat Terrain.eps',dpi=600,format='eps')
#     else:
#         plt.savefig(f'Plots of MPC Moment {labels[i]} in Flat Terrain.eps',dpi=600,format='eps')





# ---------------------------------
#           腿部关节角度
# ---------------------------------

# 读取数据
script_dir = os.path.dirname(os.path.abspath(__file__))
file_path = os.path.join(script_dir, '..', 'data', 'record_data.txt')
data = pd.read_csv(file_path, delimiter=',', header=None)

time = data.index * 0.001 

print(data.index)

# left_leg
# traget_position = data.iloc[int(0.0/0.001):, 31:37]
# actual_position = data.iloc[int(0.0/0.001):, 19:25]

# right_leg
actual_position = data.iloc[int(0.0/0.001):, 24:30]
traget_position = data.iloc[int(0.0/0.001):, 54:60]


labels = [
    'Joint1',
    'Joint2',
    'Joint3',
    'Joint4',
    'Joint5',
    'Joint6'
]

labels1 = [
    'traget position joint1',
    'traget position joint2',
    'traget position joint3',
    'traget position joint4',
    'traget position joint5',
    'traget position joint6'
]


labels2 = [
    'actual position joint1',
    'actual position joint2',
    'actual position joint3',
    'actual position joint4',
    'actual position joint5',
    'actual position joint6'
]

# labels2 = [
#     'error position joint1',
#     'error position joint2',
#     'error position joint3',
#     'error position joint4',
#     'error position joint5',
#     'error position joint6'
# ]

fig, axs = plt.subplots(3, 2, figsize=(12, 18)) 
for i in range(6):
    axs.T.flat[i].plot(time[:10000], traget_position.iloc[:10000, i], label=labels1[i])
    axs.T.flat[i].plot(time[:10000], actual_position.iloc[:10000, i], label=labels2[i])
    # axs.T.flat[i].plot(time[:10000], error_position.iloc[:10000, i], label=labels2[i])
    # axs.T.flat[i].plot(time, traget_position.iloc[:, i], label=labels1[i])
    # axs.T.flat[i].plot(time, actual_position.iloc[:, i], label=labels2[i])
    # axs.T.flat[i].set_title(titles[i])
    axs.T.flat[i].set_xlabel('time (s)', fontsize=28)
    axs.T.flat[i].set_ylabel('position (rad)', fontsize=28)
    axs.T.flat[i].legend( fontsize=16)
    axs.T.flat[i].grid()
    # axs.T.flat[i].set_ylim([-2, 2])
    axs.T.flat[i].tick_params(axis='both', which='major', labelsize=28)  # 改变横纵坐标刻度的字体大小

# plt.suptitle('Left Leg Joint Position', fontsize=16)
# plt.suptitle('Right Leg Joint Position Tracking', fontsize=16)
plt.tight_layout(rect=[0, 0, 1, 0.96])  # 调整布局以避免标题和子图重叠
# plt.savefig('Right Leg Joint Position Tracking.eps',dpi=600,format='eps')



# # 小图
# for i in range(6):
#     fig, axs = plt.subplots(figsize=(12, 6))  # 为每个子图创建单独的图
#     axs.plot(time[:24000], traget_q.iloc[0:15000, i], label=labels1[i])
#     axs.plot(time[:24000], actual_q.iloc[0:15000, i], label=labels2[i])
#     axs.set_xlabel('time (s)', fontsize=28)
#     axs.set_ylabel('torque (N·m)', fontsize=28)
#     axs.legend(fontsize=16)
#     axs.grid()
#     axs.tick_params(axis='both', which='major', labelsize=28)  # 改变横纵坐标刻度的字体大小
#     plt.tight_layout()  # 调整布局以避免标题和子图重叠
#     plt.savefig(f'Right Leg {labels[i]} Torque Tracking.eps',dpi=600,format='eps')



# ---------------------------------
#           跟踪误差
# ---------------------------------

# 读取数据
script_dir = os.path.dirname(os.path.abspath(__file__))
file_path = os.path.join(script_dir, '..', 'data', 'record_data.txt')
data = pd.read_csv(file_path, delimiter=',', header=None)

time = data.index * 0.001 

print(data.index)


error_position = data.iloc[int(0.0/0.001):, 84:90]


labels = [
    'Joint1',
    'Joint2',
    'Joint3',
    'Joint4',
    'Joint5',
    'Joint6'
]


fig, axs = plt.subplots(3, 2, figsize=(12, 18)) 
for i in range(6):
    axs.T.flat[i].plot(time[:10000], error_position.iloc[:10000, i], label=labels[i])
    axs.T.flat[i].set_xlabel('time (s)', fontsize=28)
    axs.T.flat[i].set_ylabel('position (rad)', fontsize=28)
    axs.T.flat[i].legend( fontsize=16)
    axs.T.flat[i].grid()
    # axs.T.flat[i].set_ylim([-2, 2])
    axs.T.flat[i].tick_params(axis='both', which='major', labelsize=28)  # 改变横纵坐标刻度的字体大小

# plt.suptitle('Left Leg error_position', fontsize=16)
plt.suptitle('Right Leg Joint error_position', fontsize=16)
plt.tight_layout(rect=[0, 0, 1, 0.96])  # 调整布局以避免标题和子图重叠
# plt.savefig('Right Leg Joint Position Tracking.eps',dpi=600,format='eps')



# ---------------------------------
#           不确定性估计值
# ---------------------------------

# 读取数据
script_dir = os.path.dirname(os.path.abspath(__file__))
file_path = os.path.join(script_dir, '..', 'data', 'record_data.txt')
data = pd.read_csv(file_path, delimiter=',', header=None)

time = data.index * 0.001 

print(data.index)

d_estimated = data.iloc[int(0.0/0.001):, 114:120]
# d_estimated = data.iloc[int(0.0/0.001):, 144:150]


labels = [
    'Joint1',
    'Joint2',
    'Joint3',
    'Joint4',
    'Joint5',
    'Joint6'
]


fig, axs = plt.subplots(3, 2, figsize=(12, 18)) 
for i in range(6):
    axs.T.flat[i].plot(time[:10000], d_estimated.iloc[:10000, i], label=labels[i])
    axs.T.flat[i].set_xlabel('time (s)', fontsize=28)
    axs.T.flat[i].set_ylabel('torque (N.m)', fontsize=28)
    axs.T.flat[i].legend( fontsize=16)
    axs.T.flat[i].grid()
    # axs.T.flat[i].set_ylim([-2, 2])
    axs.T.flat[i].tick_params(axis='both', which='major', labelsize=28)  # 改变横纵坐标刻度的字体大小

# plt.suptitle('Left Leg d_estimated', fontsize=16)
plt.suptitle('Right Leg Joint d_estimated', fontsize=16)
plt.tight_layout(rect=[0, 0, 1, 0.96])  # 调整布局以避免标题和子图重叠
# plt.savefig('Right Leg Joint Position Tracking.eps',dpi=600,format='eps')




# 保存图像
# plt.savefig('robot_joint_angles.png')
plt.show()
